
#pragma warning (disable : 4251 4996 4819 4244)
#include <new>
#include <stdexcept>
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <sstream>
#include <ctime>
#include <boost/shared_array.hpp>
#include <boost/random.hpp>
#include <boost/random/random_device.hpp>
#include <libplayerc++/playerc++.h>
#include "MapProxyStateValidityChecker.h"
#include "StateRecorder.h"
#include "Order.h"
#include "Executor.h"
#include "PlayerObjectManager.h"
#include "pause.h"

using namespace PlayerCc;
using namespace ompl::base;
using namespace ompl::geometric;


static const double MAX_PLAN_TIME = 0.01;

class Controller {

  boost::random::random_device rng;
  boost::random::uniform_int_distribution<> coin;

  // player parameters
  PlayerClient    robot;
  SimulationProxy sim;
  MapProxy        map;
  Position2dProxy pp;
  Position2dProxy gocon;
  Graphics2dProxy g;

  // ompl parameters
  StateSpacePtr space;
  SimpleSetup ss;

  // standby pos is robot start
  double standbyX;
  double standbyY;

  // tables
  const std::vector<PlayerObject> & tables;

  // order simulator
  std::auto_ptr<customer::OrderSimulator> ordersim;

  double reachDist;

public:

  Controller() :
    coin(0,1),
    robot("localhost"),
    sim(&robot, 0),
    map(&robot, 0),
    pp(&robot, 0),
    gocon(&robot, 100),
    g(&robot, 0),
    space(new SE2StateSpace()),
    ss(space),
    tables(PlayerObjectManager::Instance().GetStaticObjects()),
    reachDist(0.5)
  {
    robot.Read();
    map.RequestMap();

    // set bounds
    double xmax = map.GetWidth() * map.GetResolution() / 2.0;
    double ymax = map.GetHeight() * map.GetResolution() / 2.0;
    RealVectorBounds bounds(2);
    bounds.setLow(0, -xmax); bounds.setHigh(0, xmax);
    bounds.setLow(1, -ymax); bounds.setHigh(1, ymax);
    space->as<SE2StateSpace>()->setBounds(bounds);

    // set validity checker
    StateValidityCheckerPtr checker(new MapProxyStateValidityChecker(0.4, ss.getSpaceInformation(), map));
    ss.setStateValidityChecker(checker);

    // standby position is start pos
    standbyX = pp.GetXPos();
    standbyY = pp.GetYPos();
    std::cout << "Standby Pos(" << standbyX << ", " << standbyY << ")" <<std::endl;

    // get tables and init order sim
    ordersim.reset(new customer::OrderSimulator(tables.size()));
  }

private:

  PlannerPtr getPlanner() {
    PlannerPtr planner(new RRT(ss.getSpaceInformation()));
    planner->as<RRT>()->setRange(0.5);
    return planner;
  }

  template<class T>
  void record_states(T key) {
      std::stringstream stmFile;
      stmFile << "GoTo_" << key << ".csv"; 
      record_state( ss.getSolutionPath(), stmFile.str() );
      PlannerData oPlannerData( ss.getSpaceInformation() );
      ss.getPlanner()->getPlannerData( oPlannerData );
      std::vector<State*> vecStates;
      for ( unsigned int nLoop = 0; nLoop < oPlannerData.numVertices(); ++nLoop )
      {
          vecStates.push_back((State*)(oPlannerData.getVertex( nLoop ).getState()));
      }
      record_state( vecStates, stmFile.str() + ".plantree.csv" );
  }

  // plan motion from one start to one goal and execute
  void planAndExecute(const std::string & description, double goalX, double goalY) {
    // get current robot positions etc.
    robot.Read();

    // start at the existing position
    ScopedState<SE2StateSpace> start(ss.getSpaceInformation());
    start->setX(pp.GetXPos()); start->setY(pp.GetYPos());

    // goal is the standby position
    ScopedState<SE2StateSpace> goal(ss.getSpaceInformation());
    goal->setX(goalX); goal->setY(goalY);

    // visualize
    std::cout << description.c_str() << " (" << goalX << ", " << goalY << ")" << std::endl;

    // set start and goal
    ss.setStartAndGoalStates(start, goal);

    // setup the path planner
    ss.setPlanner(getPlanner());
    ss.clear();
    ss.setup();

    // plan
    PlannerStatus solved = ss.solve();

    // execute
    if (solved) {
       ss.simplifySolution();
       //record_states(description);
       Executor executor(ss.getSolutionPath(), robot, pp, gocon);
       executor.execute();
       util::pause(5);
    }

    std::cout << description.c_str() << " (" << goalX << ", " << goalY << ") " << (solved? "done": "failed!") << std::endl;
    

  }

  void standby() {
    planAndExecute("Standby", standbyX, standbyY);
  }

  // generate a goal for a particular table, not necessary valid
  std::pair<double,double> generateGoal(const PlayerObject & table) {
    double x, y;

    // generate X
    if (coin(rng)) {
      boost::uniform_real<> u(table.PosLowX - reachDist, table.PosLowX);
      x = u(rng);
    } else {
      boost::uniform_real<> u(table.PosHighX, table.PosHighX + reachDist);
      x = u(rng);
    }

    // generate Y
    if (coin(rng)) {
      boost::uniform_real<> u(table.PosLowY - reachDist, table.PosLowY);
      y = u(rng);
    } else {
      boost::uniform_real<> u(table.PosHighY, table.PosHighY + reachDist);
      y = u(rng);
    }

    return std::make_pair(x,y);
  }

  // generates a valid goal position
  std::pair<double,double> generateValidGoal(int tableIndex) {
    const PlayerObject & table = tables[tableIndex];
    for (int i = 0; i < 100; ++i) {
      std::pair<double,double> gg = generateGoal(table);
      // check state valid
      ScopedState<SE2StateSpace> cg(ss.getSpaceInformation());
      cg->setX(gg.first); cg->setY(gg.second);
      if (ss.getStateValidityChecker()->isValid(cg.get())) {
        return gg;
      }
    }
    throw std::runtime_error("no valid goal position!");
  }

  void deliver(const std::vector<customer::Order> orders) {

    int deliverSize = orders.size();
    int deliverNum = 1;

    std::cout << "Delivering " << deliverSize << " orders" << std::endl;

    for (std::vector<customer::Order>::const_iterator i = orders.begin();
      i != orders.end(); ++i) {

      std::string tableName = tables[i->table].Name;
      std::pair<double,double> goal = generateValidGoal(i->table);

      //std::cout << "Delivering #" << deliverNum << "/" << deliverSize << " to "<< i->table << " (" << tableName.c_str() << ")" << std::endl;
      //std::cout << "-> goal (" << nearTable.first << ", " << nearTable.second << ")" << std::endl;

      std::stringstream description;
      description << "Deliver_" << deliverNum++ << "_of_" << deliverSize << "_to_" << tableName;
      planAndExecute(description.str(), goal.first, goal.second);
    }
  }

public:

  // control loop
  void run() {

    for (;;) {
      // generate a list of orders to fulfil
      std::vector<customer::Order> orders = ordersim->getOrders();

      // if no orders then standby, else deliver them
      if (!orders.empty()) {
        deliver(orders);
        standby();
      }
    }

  }
};

int
main(int argc, char *argv[])
{
    std::vector<std::string> vecDefFiles;
    vecDefFiles.push_back( "../worlds/tables.inc" );
    vecDefFiles.push_back( "../worlds/person.inc" );
    PlayerObjectManager::Instance().ParseConfigFile( vecDefFiles, "../worlds/simple.world" );
  try {
    Controller control;
    control.run();
    return 0;
  } catch (std::exception &e) {
    std::cerr << e.what() << std::endl;
    return 1;
  }
}
